#include "Robot2489.h"

//Use buttons 2 and 3 to move the conveyor belt up and down, respectively
void Robot2489::ControlConveyorBelt(void){
	if(gameStick->GetRawButton(8)){
		conveyorBeltMotor->Set(0.8);
	}
	else if (gameStick->GetRawButton(7)){
		
		
		conveyorBeltMotor->Set(-0.8);
	}
	else{
		conveyorBeltMotor->Set(0.0);
	}
	screen->UpdateLCD();
}

//Use manipulator joystick to turn the turret.
void Robot2489::ControlTurretTurn(void){
	//check whether either of the limit switches have been hit
	rightLimit = !rightTurretLimit->Get();
	leftLimit = !leftTurretLimit->Get();
	if (!autoTurn){                                 ////Manual control of joystick////
		
/*		if (rightLimit){
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "right");
		}
		else if (leftLimit){
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "left");
		}
		else {	
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "none");
		}*/
		//turretValue = manipStick->GetX(); // gets joystick value
		turretValue = gameStick->GetRawAxis(4); 
		turretValue = LogisticSpeedProgression(turretValue);
	
		//define deadzone, and move only if it's between the limit switches
		//screen->PrintfLine(DriverStationLCD::kUser_Line4, "turretValue: %f", turretValue);
		if (turretValue <= 0.01 && turretValue >= -0.01) turretValue = 0.0;
		if (rightLimit && turretValue >= 0.0){
			turretMotor->Set(turretValue);
		}
		else if (leftLimit && turretValue <= 0.0){
			turretMotor->Set(turretValue);
		}
		else if (!leftLimit && !rightLimit){
			turretMotor->Set(turretValue);
		}
		else;
	}
	
	
	else{										///autonomous control///
		
		//check the angle
		rotAngle += ROTVELOCITYPERSIGNAL*0.2*(timer->Get() - prevTime);
		turretMotor->Set(0.2);
		//stop after reaching target angle
		if (fabs(turretAngle) <= fabs(rotAngle)){
			turretMotor->Set(0.0);
			autoTurn=false;
		}
		
		screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f", rotAngle);
		//get current time for integral control
		prevTime = timer->Get();
	}
}

//If trigger is pressed, fire the turret with current wheel speeds
void Robot2489::ControlTurretFiring(void){
	double voltageScaling = 12.0 / station->GetBatteryVoltage();
	if(manipStick->GetRawButton(3)){
		
	//	topTurretWheel->Set(-0.9 * bottomWheelValue*voltageScaling);
		bottomTurretWheel->Set(bottomWheelValue*voltageScaling);
	//	previousTurretFire = true;
	}
	else{
	//	topTurretWheel->Set(0);
		bottomTurretWheel->Set(0);
		
	//	utilityTimer->Stop();
	//	utilityTimer->Reset();
	//	previousTurretFire = false;
	}
	
	/*
	topTurretWheel->Set(
	*/

}

//Control the ramp tool (button 7 to move it down, button 6 to return it to
//rest position).

void Robot2489::StartAutoAimTurret(void)
{	
	//if (reportCount == 1){
		turretAngle = 6.0;
		rotAngle = 0;
		//rotSpeed = 0.2;   Assumed true by default, shows up as .2
		prevTime = timer->Get();
		turretMotor->Set(0.2);
		autoTurn = true;
	//}
}


void Robot2489::ResetTurret(void){
	
	//turn the motor all the way to the right
	while (rightTurretLimit->Get()){
		turretMotor->Set(0.7);
		Wait(0.005);
	}
	turretMotor->Set(0.0);
	
	//reset timer
	utilityTimer->Reset();
	utilityTimer->Start();

	//turn it back towards the left limit switch, keeping track of the time
	while (leftTurretLimit->Get()){
		turretMotor->Set(-0.7);
		Wait(0.005);
	}
	double time = utilityTimer->Get();
	turretMotor->Set(0.0);
	
	//turn motor back in half the time, hopefully turning it exactly halfway
	utilityTimer->Reset();
	while (utilityTimer->Get() < time){
		turretMotor->Set(0.7);
		Wait(0.005);
	}
	turretMotor->Set(0.0);
	utilityTimer->Stop();
}

void Robot2489::DisplayGearToothVelocity(){
	if(newInterval){
		tIntial = timer->Get();
		xIntial = gearToothCounter->Get();
		newInterval = 0;
	}else{
		double currentTime;
		currentTime = timer->Get();
		double checkInterval;
		checkInterval = currentTime - tIntial;
		if(checkInterval >= 0.3){
			int currentX;
			currentX = gearToothCounter->Get();
			double currentSpeed;
			currentSpeed = (currentX- xIntial)/checkInterval;
			screen->PrintfLine(DriverStationLCD::kUser_Line5,"Cnt: %d, Sd: %d",
					gearToothCounter->Get(), (((int) currentSpeed)/5)*5);
			screen->UpdateLCD();
			newInterval = 1;
		}
	}
}
